using System.Collections;

namespace Commons.Collections 
{
	public class PriorityQueue 
	{
		private object[] _nodes;
		private Hashtable _n2i;
		private int _count;
		
		public PriorityQueue(int capacity)
		{
			_count = 0;
			_nodes = new object[capacity];
			_n2i   = new Hashtable();
		}
		
		private int LeftChildIndex(int parent)
		{
			return (parent << 1)+1;
		}
		
		private int RightChildIndex(int parent)
		{
			return (parent << 1) + 2;
		}
		
		private int ParentIndex(int child)
		{
			return (child & ~1) >>  1;
		}
		
		public int count
		{
			get { return _count; }
		}
		
		public bool Empty()
		{
			return _count == 0;
		}
		
		private void SetNode(int i, object node)
		{
//			Debug.Log("set node " + i );
			_nodes[i] = node;
			_n2i[((Hashtable)node)["data"]] = i;
		}
		
		public void Add(object obj, int priority)
		{
			if ( _count == _nodes.Length )
				Debug.LogError("Heap::add: heap overflow.");
			
			Hashtable entry = new Hashtable();
			entry["data"] = obj;
			entry["p"] = priority;
			SetNode(count,  entry );
			ShiftUp(count);
			_count++;
		}
		
		private int MinChild(int parentIndex)
		{
			int ileft = LeftChildIndex(parentIndex);
			int iright = RightChildIndex(parentIndex);
			Hashtable left = (Hashtable)_nodes[ileft];
			Hashtable right = (Hashtable)_nodes[iright];
			
			if ( right == null )
				return ileft;
			
			return ( (int)left["p"] < (int)right["p"] ) ? ileft : iright;
		}
		
		private void SwapNodes(int i1, int i2)
		{
			object tmp = _nodes[i1];
			
			SetNode(i1, _nodes[i2]);
			SetNode(i2, tmp);
		}
		
		private void ShiftUp(int index)
		{
			if (index == 0)
				return;
			
			int parentIdx = ParentIndex(index);
			int childIdx  = MinChild(parentIdx);
			Hashtable child  = (Hashtable) _nodes[childIdx];
			Hashtable parent = (Hashtable) _nodes[parentIdx];
			if(child == null) return;
			
			if ( (int)child["p"] < (int)parent["p"] )
			{
				SwapNodes(parentIdx, childIdx);
				ShiftUp(parentIdx);
			}
		}
		
		private bool IsLeaf(int index)
		{
			return (LeftChildIndex(index) >= _count);
		}
		
		private void ShiftDown(int index)
		{
			if (index >= _count || IsLeaf(index))
				return;
			
			int childIdx = MinChild(index);
			Hashtable child = (Hashtable) _nodes[childIdx];
			Hashtable parent = (Hashtable) _nodes[index];
			if(child == null)return;
			
			if ( (int)child["p"] < (int)parent["p"] )
			{
				SwapNodes(index, childIdx);
				ShiftDown(childIdx);
			}
		}
		
		public object Shift()
		{
			if (count == 0)
				Debug.LogError("Heap::shift, heap underflow.");
			
			object obj = ((Hashtable)_nodes[0])["data"];
			
			SetNode(0,_nodes[_count - 1]);
			_nodes[_count-1] = null;
			_n2i.Remove(obj);
			
			_count--;
			ShiftDown(0);
			
			return obj;
		}
		
		public object Top()
		{
			if (count == 0)
				Debug.LogError("Heap::shift, heap underflow.");
			
			return ((Hashtable)_nodes[0])["data"];
		}
		
		public bool IsNodeAdded(object obj)
		{
			return _n2i.ContainsKey(obj);
		}
		
		public void Reprioritize(object obj, int newPriority)
		{
			object idx = _n2i[obj];
			
			if ( idx == null )
				 Debug.LogError("Heap::reprioritize, object does not exist.");
			
			Hashtable entry = (Hashtable)_nodes[(int)idx];
			entry["p"] = newPriority;
			ShiftDown((int)idx);
			ShiftUp((int)idx);
		}
	}
}
